#pragma config(Hubs,  S1, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Motor,  mtr_S1_C2_1,     mot_BB,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     mot_WL,        tmotorNormal, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     mot_SL,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     mot_WR,        tmotorNormal, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C1_1,    ser_CL,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_2,    ser_CR,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_3,    ser_CAL,              tServoStandard)
#pragma config(Servo,  srvo_S1_C1_4,    ser_CAR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C1_5,    ser_BR,               tServoStandard)
#pragma config(Servo,  srvo_S1_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

task main()
{
  int threshold = 18;//Stop low readings from moving the robot

  /*
  CONTROL CODE START
  */
  while(true)//infinite loop
  {
    //TANK DRIVE
    getJoystickSettings(joystick);
    if(abs(joystick.joy1_y2) > threshold)//Check if reading is small enough to ignore
    {
      motor[mot_WR] = joystick.joy1_y2;
    }
    else
    {
      motor[mot_WR] = 0;
    }


    if(abs(joystick.joy1_y1) > threshold)//Check if reading is small enough to ignore
    {
      motor[mot_WL] = joystick.joy1_y1;
    }
    else
    {
      motor[mot_WL] = 0;
    }
    //TANK DRIVE END

    //SCISSOR LIFT
    if(joy1Btn(07) == 1)//Right Trigger -> Up
    {
      motor[mot_SL] = 20;
    }
    else if(joy1Btn(06) == 1)//Left Trigger -> Down
    {
      motor[mot_SL] = -20;
    }
    else
    {
      motor[mot_SL] = 0;
    }
    //SCISSOR LIFT END

    //CLAW OPEN/CLOSE
    if((joy1Btn(05) == 1) && (servo[ser_CAL]+3 <= 255))
    {
      servo[ser_CAL] += 3;
      servo[ser_CAR] = servo[ser_CAL];
    }
    else if((joy1Btn(04) == 1) && (servo[ser_CAL]-3 >= 0))
    {
      servo[ser_CAL] -= 3;
      servo[ser_CAR] = servo[ser_CAL];
    }
    //CLAW OPEN/CLOSE END

    //CLAW SPIN
    if((joystick.joy1_TopHat == 0) && (servo[ser_CL]+3 <= 255))
    {
      servo[ser_CL] += 3;
      servo[ser_CR] = servo[ser_CL];
    }
    else if((joystick.joy1_TopHat) == 4 && (servo[ser_CL]-3 >= 0))
    {
      servo[ser_CL] -= 3;
      servo[ser_CR] = servo[ser_CL];
    }
    //CLAW SPIN END
    /*
    //BALL RELEASE
    if(joy2Btn(07) == 1)//Right Trigger for Player 2
    {
      servo[ser_BR] = 128;//Open Tube
    }
    else
    {
      servo[ser_BR] = 0;//Close Tube
    }
    //BALL RELEASE END

    //CHAIN STOP
    if(joy2Btn(06) == 1)//Left Trigger for Player 2
    {
      motor[mot_BB] = 0;//Chain Stop
    }
    else
    {
      motor[mot_BB] = 40;//Chain On
    }
    //CHAIN STOP END

    //Just For Fun (For Player 2)
    if(joy2Btn(00) == 1)
    {
      PlaySoundFile("alarm_beep.rso");
    }
    if(joy2Btn(01) == 1)
    {
      PlaySoundFile("baby_x.rso");
    }
    if(joy2Btn(02) == 1)
    {
      PlaySoundFile("babycry.rso");
    }
    if(joy2Btn(03) == 1)
    {
      PlaySoundFile("babycry2.rso");
    }
    //JFF END
    */
    //END OF CODE
  }
}
